#include "stdafx.h"

#include <cv.h>
#include <highgui.h>

IplImage* imgTracking;
int lastX = -1;
int lastY = -1;

IplImage* imgTracking1;
int lastX1 = -1;
int lastY1 = -1;

IplImage* imgTracking2;
int lastX2 = -1;
int lastY2 = -1;

IplImage* imgTracking3;
int lastX3 = -1;
int lastY3 = -1;

//This function threshold the HSV image and create a binary image// red colour
IplImage* GetThresholdedImage(IplImage* imgHSV){
    IplImage* imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
    cvInRangeS(imgHSV, cvScalar(170,160,60,0), cvScalar(180,255,256,0), imgThresh);
    return imgThresh;
}

//yellow
IplImage* GetThresholdedImage1(IplImage* imgHSV1){
    IplImage* imgThresh1=cvCreateImage(cvGetSize(imgHSV1),IPL_DEPTH_8U, 1);
    cvInRangeS(imgHSV1, cvScalar(22,49,120,0), cvScalar(32,255,256,0), imgThresh1);
    return imgThresh1;
}

//blue
IplImage* GetThresholdedImage2(IplImage* imgHSV2){
    IplImage* imgThresh2=cvCreateImage(cvGetSize(imgHSV2),IPL_DEPTH_8U, 1);
    cvInRangeS(imgHSV2, cvScalar(101,167,136,0), cvScalar(116,256,256,0), imgThresh2);
    return imgThresh2;
}

//green
IplImage* GetThresholdedImage3(IplImage* imgHSV3){
    IplImage* imgThresh3=cvCreateImage(cvGetSize(imgHSV3),IPL_DEPTH_8U, 1);
    cvInRangeS(imgHSV3, cvScalar(42,69,57,0), cvScalar(75,255,256,0), imgThresh3);
    return imgThresh3;
}


//red
void trackObject(IplImage* imgThresh){
// Calculate the moments of 'imgThresh'
CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
cvMoments(imgThresh, moments, 1);
double moment10 = cvGetSpatialMoment(moments, 1, 0);
double moment01 = cvGetSpatialMoment(moments, 0, 1);
double area = cvGetCentralMoment(moments, 0, 0);

     // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
if(area>1000){
        // calculate the position of the ball
int posX = moment10/area;
int posY = moment01/area;

       if(lastX>=0 && lastY>=0 && posX>=0 && posY>=0)
{
// Draw a yellow line from the previous point to the current point
cvLine(imgTracking, cvPoint(posX, posY), cvPoint(lastX, lastY), cvScalar(0,0,255,0), 4,8,0);
}

lastX = posX;
lastY = posY;
}

free(moments);
}


//yellow
void trackObject1(IplImage* imgThresh1){
// Calculate the moments of 'imgThresh'
CvMoments *moments1 = (CvMoments*)malloc(sizeof(CvMoments));
cvMoments(imgThresh1, moments1, 1);
double moment10 = cvGetSpatialMoment(moments1, 1, 0);
double moment01 = cvGetSpatialMoment(moments1, 0, 1);
double area1 = cvGetCentralMoment(moments1, 0, 0);

     // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
if(area1>1000){
        // calculate the position of the ball
int posX1 = moment10/area1;
int posY1 = moment01/area1;

       if(lastX1>=0 && lastY1>=0 && posX1>=0 && posY1>=0)
{
// Draw a yellow line from the previous point to the current point
cvLine(imgTracking1, cvPoint(posX1, posY1), cvPoint(lastX1, lastY1), cvScalar(0,0,255,0), 4,8,0);
}

lastX1 = posX1;
lastY1 = posY1;
}

free(moments1);
}


//blue
void trackObject2(IplImage* imgThresh2){
// Calculate the moments of 'imgThresh'
CvMoments *moments2 = (CvMoments*)malloc(sizeof(CvMoments));
cvMoments(imgThresh2, moments2, 1);
double moment10 = cvGetSpatialMoment(moments2, 1, 0);
double moment01 = cvGetSpatialMoment(moments2, 0, 1);
double area2 = cvGetCentralMoment(moments2, 0, 0);

     // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
if(area2>1000){
        // calculate the position of the ball
int posX2 = moment10/area2;
int posY2 = moment01/area2;

       if(lastX2>=0 && lastY2>=0 && posX2>=0 && posY2>=0)
{
// Draw a yellow line from the previous point to the current point
cvLine(imgTracking, cvPoint(posX2, posY2), cvPoint(lastX2, lastY2), cvScalar(0,0,255,0), 4,8,0);
}

lastX2 = posX2;
lastY2 = posY2;
}

free(moments2);
}



//green
void trackObject3(IplImage* imgThresh3){
// Calculate the moments of 'imgThresh'
CvMoments *moments3 = (CvMoments*)malloc(sizeof(CvMoments));
cvMoments(imgThresh3, moments3, 1);
double moment10 = cvGetSpatialMoment(moments3, 1, 0);
double moment01 = cvGetSpatialMoment(moments3, 0, 1);
double area3 = cvGetCentralMoment(moments3, 0, 0);

     // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
if(area3>1000){
        // calculate the position of the ball
int posX3 = moment10/area3;
int posY3 = moment01/area3;

       if(lastX3>=0 && lastY3>=0 && posX3>=0 && posY3>=0)
{
// Draw a yellow line from the previous point to the current point
cvLine(imgTracking3, cvPoint(posX3, posY3), cvPoint(lastX3, lastY3), cvScalar(0,0,255,0), 4,8,0);
}

lastX3 = posX3;
lastY3 = posY3;
}

free(moments3);
}




int main(){

      CvCapture* capture =0;
      int i;
      capture = cvCaptureFromCAM(1);

      if(!capture){
printf("Capture failure\n");
return -1;
      }

      IplImage* frame=0;
      frame = cvQueryFrame(capture);
      if(!frame) return -1;

     //create a blank image and assigned to 'imgTracking' which has the same size of original video
     imgTracking=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3);
     cvZero(imgTracking); //covert the image, 'imgTracking' to black

     cvNamedWindow("Video",0);
     cvNamedWindow("red",0);

     //yellow

     imgTracking1=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3);
     cvZero(imgTracking1); //covert the image, 'imgTracking' to black

     //cvNamedWindow("Video1",0);
     //cvNamedWindow("yellow",0);

     //blue

     imgTracking2=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3);
     cvZero(imgTracking2); //covert the image, 'imgTracking' to black

    // cvNamedWindow("Video2",0);
     //cvNamedWindow("blue",0);

     //

     imgTracking3=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3);
     cvZero(imgTracking3); //covert the image, 'imgTracking' to black

    // cvNamedWindow("Video3",0);
    // cvNamedWindow("green",0);


      //iterate through each frames of the video
      while(1){

            frame = cvQueryFrame(capture);
            if(!frame) break;
            frame=cvCloneImage(frame);

           cvSmooth(frame, frame, CV_GAUSSIAN,3,3,0,0); //smooth the original image using Gaussian kernel



//red colour object

            IplImage* imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
            cvCvtColor(frame, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV
            IplImage* imgThresh = GetThresholdedImage(imgHSV);

            cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,3,3,0,0); //smooth the binary image using Gaussian kernel

          //track the possition of the ball
          trackObject(imgThresh);

            // Add the tracking image and the frame
          cvAdd(frame, imgTracking, frame,0);

          cvShowImage("red", imgThresh);
           cvShowImage("Video", frame);



                IplImage* imgGrayScale=cvCreateImage(cvGetSize(imgHSV),8,1);

                 cvCvtColor(frame, imgGrayScale, CV_BGR2GRAY);
                 cvThreshold(imgGrayScale, imgGrayScale, 128, 255, CV_THRESH_BINARY);
                 CvSeq* contours;
                 CvSeq* result;
                 //printf("adc");

                 CvMemStorage* storage =cvCreateMemStorage(0);

                 //printf("leaving memstore");
                 cvFindContours(imgGrayScale, storage, &contours, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE,cvPoint(0,0));




               while(contours)
               {
                   //printf("entering while");

                 result = cvApproxPoly(contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02,0);


                    //printf("result");
                  if(result->total==4 && fabs(cvContourArea(result, CV_WHOLE_SEQ,0))>40 )
                     {
                       //iterating through each point
                            CvPoint *pt[4];
                             for(i=0;i<4;i++){
                              pt[i] = (CvPoint*)cvGetSeqElem(result, i);
                      }

                               //drawing lines around the quadrilateral
                        cvLine(frame, *pt[0], *pt[1], cvScalar(0,0,255,0),4,8,0);
                           cvLine(frame, *pt[1], *pt[2], cvScalar(0,0,255,0),4,8,0);
                        cvLine(frame, *pt[2], *pt[3], cvScalar(0,0,255,0),4,8,0);
                             cvLine(frame, *pt[3], *pt[0], cvScalar(0,0,255,0),4,8,0);


                     }


                    contours = contours->h_next;


                      //show the image in which identified shapes are marked
                       cvNamedWindow("Tracked",0);
                       cvShowImage("Tracked",frame);

                   }





//yellow colour object

 IplImage* imgHSV1 = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
            cvCvtColor(frame, imgHSV1, CV_BGR2HSV); //Change the color format from BGR to HSV
            IplImage* imgThresh1 = GetThresholdedImage1(imgHSV1);

            cvSmooth(imgThresh1, imgThresh1, CV_GAUSSIAN,3,3,0,0); //smooth the binary image using Gaussian kernel

          //track the possition of the ball
          trackObject1(imgThresh1);

            // Add the tracking image and the frame
          cvAdd(frame, imgTracking1, frame,0);

          //cvShowImage("yellow", imgThresh1);
           //cvShowImage("Video1", frame);




//blue colour object

IplImage* imgHSV2 = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
            cvCvtColor(frame, imgHSV2, CV_BGR2HSV); //Change the color format from BGR to HSV
            IplImage* imgThresh2 = GetThresholdedImage2(imgHSV2);

            cvSmooth(imgThresh2, imgThresh2, CV_GAUSSIAN,3,3,0,0); //smooth the binary image using Gaussian kernel

          //track the possition of the ball
          trackObject2(imgThresh2);

            // Add the tracking image and the frame
          cvAdd(frame, imgTracking2, frame,0);

          //cvShowImage("blue", imgThresh2);
          // cvShowImage("Video2", frame);



//green colour object

IplImage* imgHSV3 = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
            cvCvtColor(frame, imgHSV3, CV_BGR2HSV); //Change the color format from BGR to HSV
            IplImage* imgThresh3 = GetThresholdedImage3(imgHSV3);

            cvSmooth(imgThresh3, imgThresh3, CV_GAUSSIAN,3,3,0,0); //smooth the binary image using Gaussian kernel

          //track the possition of the ball
          trackObject3(imgThresh3);

            // Add the tracking image and the frame
          cvAdd(frame, imgTracking3, frame,0);

          //cvShowImage("green", imgThresh3);
           //cvShowImage("Video3", frame);




           //Clean up used images
           cvReleaseImage(&imgHSV);
           cvReleaseImage(&imgThresh);

           cvReleaseImage(&imgHSV1);
           cvReleaseImage(&imgThresh1);

           cvReleaseImage(&imgHSV2);
           cvReleaseImage(&imgThresh2);

           cvReleaseImage(&imgHSV3);
           cvReleaseImage(&imgThresh3);


           cvReleaseImage(&frame);

            //Wait 10mS
            int c = cvWaitKey(10);
            //If 'ESC' is pressed, break the loop
            if((char)c==27 ) break;
      }

      cvDestroyAllWindows() ;
      cvReleaseImage(&imgTracking);
      cvReleaseCapture(&capture);

      return 0;
}
